#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Int16.h>
#include <sensor_msgs/Imu.h>

// 计算校验和
uint8_t add8(uint8_t* ser, int nums)
{
    uint8_t add = 0;
    for(int t = 0; t < nums; t++)
        add += ser[t];
    return add;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "serial_read_node");
    ros::NodeHandle nh;

    // 创建两个publisher
    ros::Publisher encoder_pub = nh.advertise<std_msgs::Int16>("encoder_data", 1000);
    ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu_data", 1000);

    // 打开串口
    serial::Serial ser;
    try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB0"); 
        ser.setBaudrate(921600); 
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 

    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    } 

    // 循环读取串口数据并解析
    while(ros::ok())
    {
        if(ser.available())
        {
            std_msgs::String data;
            data.data = ser.read(ser.available());

            uint8_t* byte_data = reinterpret_cast<uint8_t*>(&data.data[0]);
            int length = data.data.length();
            // 判断数据长度是否为8
            if (length == 8)
            {
                // 判断数据头是否正确
                if (byte_data[0] == 0xAA && byte_data[1] == 0xAF && byte_data[2] == 0x01)
                {
                    uint8_t check_sum = byte_data[7];
                    int16_t encoder_data = (byte_data[5] << 8) | byte_data[6];
                    // 校验和是否正确
                    if (check_sum == add8(byte_data, 7))
                    {
                        std_msgs::Int16 msg;
                        msg.data = encoder_data;
                        encoder_pub.publish(msg);
                    }
                }
            }
            // 判断数据长度是否为18
            else if (length == 18)
            {
                // 判断数据头是否正确
                if (byte_data[0] == 0xAA && byte_data[1] == 0xAF && byte_data[2] == 0x01)
                {
                    uint8_t check_sum = byte_data[17];
                    float x_acc = (int16_t)((byte_data[5] << 8) | byte_data[6])/1000.0;
                    float y_acc = (int16_t)((byte_data[7] << 8) | byte_data[8])/1000.0;
                    float z_acc = (int16_t)((byte_data[9] << 8) | byte_data[10])/1000.0;
                    float x_gyro = (int16_t)((byte_data[11] << 8) | byte_data[12])/100.0;
                    float y_gyro = (int16_t)((byte_data[13] << 8) | byte_data[14])/100.0;
                    float z_gyro = (int16_t)((byte_data[15] << 8) | byte_data[16])/100.0;
                    // 校验和是否正确
                    if (check_sum == add8(byte_data, 17))
                    {
                        sensor_msgs::Imu imu_data;
                        imu_data.header.stamp = ros::Time::now();
                        imu_data.header.frame_id = "base_link";

                        imu_data.angular_velocity.x = x_acc;
                        imu_data.angular_velocity.y = y_acc;
                        imu_data.angular_velocity.z = z_acc;
                
                        imu_data.linear_acceleration.x = x_gyro;
                        imu_data.linear_acceleration.y = y_gyro;
                        imu_data.linear_acceleration.z = z_gyro;

                        imu_pub.publish(imu_data);
                    }
                }
            }
        }
        ros::spinOnce();
    }
    return 0;
}
